﻿#include "filenamedialog.h"
#include "CircleFitSolver.h"
#include "tabledisplay.h"
#include "batchthread.h"
#include "mainwindow.h"
#include "DisplayWidget/DisplayWidget.h"
#include <QDebug>

BatchThread::BatchThread(QObject *parent)
    :QThread(parent),
      m_stop(false),
      m_autoFocus(false),
      m_autoCenter(false),
      m_autoMeasure(false),
      m_xSemaphore(0),
      m_ySemaphore(0),
      m_zSemaphore(0)
{
    m_win = NULL;

    m_fileNames.reserve(40);
    m_rows.reserve(40);
    m_cols.reserve(40);
    m_posXmm.reserve(40);
    m_posYmm.reserve(40);

}

void BatchThread::xMoveFinished()
{
    if(m_xSemaphore.available() == 0 )
    {
        qDebug() << "BatchThread::xMoveFinished()";
        m_xSemaphore.release();
    }
}

void BatchThread::yMoveFinished()
{
    if(m_ySemaphore.available() == 0)
    {
        m_ySemaphore.release();
    }
}

void BatchThread::zMoveFinished()
{
    if(m_zSemaphore.available() == 0)
    {
        m_zSemaphore.release();
    }
}

void BatchThread::addMeasurePoint(int row, int col, double x_mm, double y_mm, QString fileName)
{
    m_rows.append(row);
    m_cols.append(col);
    m_posXmm.append(x_mm);
    m_posYmm.append(y_mm);
    m_fileNames.append(fileName);
}

void BatchThread::resetMeasurePoints()
{
    m_fileNames.clear();
    m_rows.clear();
    m_cols.clear();
    m_posXmm.clear();
    m_posYmm.clear();

    m_zList.clear();
}

void BatchThread::setMainWindow(MainWindow *win)
{
    m_win = win;
}

BatchThread::~BatchThread()
{

}
void BatchThread::initSemaphore() // 确保每个 semaphore 都为 0
{
    int x = m_xSemaphore.available();
    int y = m_ySemaphore.available();
    int z = m_zSemaphore.available();
    if(x)
    {
        m_xSemaphore.acquire(x);
    }
    if(y)
    {
        m_ySemaphore.acquire(y);
    }
    if(z)
    {
        m_zSemaphore.acquire(z);
    }
}

void BatchThread::setZPositions(QList<double> zList)
{
    m_zList = zList;
}

void BatchThread::run()
{
    m_stop = false;
    initSemaphore();
    while( !m_posXmm.empty())
    {
        if(m_stop) break;
        double x_mm = m_posXmm.takeFirst();
        double y_mm = m_posYmm.takeFirst();
        int row = m_rows.takeFirst();
        int col = m_cols.takeFirst();

        qDebug() << "run: row = " << row << ", col = " << col;
        QString name = m_fileNames.takeFirst();

        emit moveToPos(x_mm, y_mm, 5.0);
        if( !waitGotoPos() )
        {
            break;
        }
        qDebug() << "move Finished!";

        // 多停一会儿确保平台的振动已经消减
        msleep(200);
        if(m_autoFocus) autoFocus();
        if(m_autoCenter) autoCenter();
        msleep(200);
        if(m_autoMeasure) autoMeasure();

        qDebug() << "saveImage";
        m_win->saveImage(row, col, m_autoMeasure);
    }
    emit batchMeasureFinish();
}

bool BatchThread::waitGotoPos()
{
    if( !m_xSemaphore.tryAcquire(1, 10*1000) )
    {
        qWarning() << "xSemaphore timeout, Batch Measure failed!";
        return false;
    }
    if( !m_ySemaphore.tryAcquire(1, 10*1000) )
    {
        qWarning() << "ySemaphore timeout, Batch Measure failed!";
        return false;
    }
    return true;
}

bool BatchThread::waitingForZStop()
{
    if( !m_zSemaphore.tryAcquire(1, 10*1000) )
    {
        qWarning() << "zSemaphore timeout, Batch Measure failed!";
        return false;
    }
    return true;
}


void BatchThread::stop()
{
    m_stop = true;
    xMoveFinished();
    yMoveFinished();
    zMoveFinished();
}

void BatchThread::autoCenter()
{
    QImage image = m_win->getImage();
    cv::Mat mat = QImage2cvMat(image);
    cv::Mat grayImage;
    cv::cvtColor(mat, grayImage, CV_BGR2GRAY);
    cv::threshold(grayImage, grayImage, 0, 255, CV_THRESH_OTSU);
    cv::Canny(grayImage, grayImage,  100, 200, 7);

    std::vector<cv::Point2i> points;

    for(int row = 0; row < grayImage.rows; row++)
    {
        const uchar * pLine = grayImage.ptr<const uchar>(row);
        for(int col = 0; col < grayImage.cols; col++)
        {
            if(pLine[col] > 128)
            {
                points.push_back(cv::Point2i(col, row));
            }
        }
    }

    CircleFitSolver solver;
    double x, y, center;
    if( solver.circleFitL1(points, x, y, center) )
    {
        double image_center_x = image.width() / 2.0;
        double image_center_y = image.height() / 2.0;

        x = image_center_x - x;
        y = image_center_y - y;

        x = x * m_scale; // 换算成 mm
        y = y * m_scale; // 换算成 mm

        emit moveToRelPos(x, y, 5.0);
        waitGotoPos();
    }
}

void BatchThread::autoMeasure()
{
    m_win->clearAllMark();
    m_win->autoMeasure();
}

void BatchThread::autoFocus()
{
    double best_score = -1;
    double z_best = m_zList[0];
    for(int i = 0; i < m_zList.size(); i++)
    {
        double z_mm = m_zList[i];

        emit zMoveToPos(z_mm, 0.5);
        if( !waitingForZStop() )
        {
            return;
        }
        msleep(50);
        QImage image = m_win->getImage();
        cv::Mat mat = m_focusMeasure.preprocess(image);
        double score = m_focusMeasure.measure(mat);
        qDebug() << "autoFocus z = " << z_mm << ", score = " << score;
        if(score > best_score)
        {
            best_score = score ;
            z_best = z_mm;
        }
    }
    qDebug() << "best z = " << z_best;
    emit zMoveToPos(z_best, 0.5);
    waitingForZStop();
}

